#include <GL/gl.h>
#include <GL/glu.h>
#include <SDL/SDL.h>
#include <math.h>
#include "MRMesh.h"
#include <iostream>
using namespace std;

void MRSection::draw(int d, d_Frustum *frustum, bool visible)
{
	if(visible)
	{
		if(d==level)
		{
				for(int i=0; i<num_splats; i++)
				{
					glNormal3f(splats[i].n[0], splats[i].n[1], splats[i].n[2]);
					glColor3f(splats[i].c[0], splats[i].c[1], splats[i].c[2]);
					glVertex3f(splats[i].vp[0], splats[i].vp[1], splats[i].vp[2]);
				}
		}
		else
		{
			for (int i=0; i<num_childs; i++) 
					childs[i]->draw(d, frustum, true);
		}
		return;
	}

	if(frustum->boxInFrustum(aabb) == INSIDE)
		if(is_front_facing(frustum))
		{
			if(d==level)
			{
					for(int i=0; i<num_splats; i++)
					{
						glNormal3f(splats[i].n[0], splats[i].n[1], splats[i].n[2]);
						glColor3f(splats[i].c[0], splats[i].c[1], splats[i].c[2]);
						glVertex3f(splats[i].vp[0], splats[i].vp[1], splats[i].vp[2]);
					}
			}
			else
			{
				for (int i=0; i<num_childs; i++) 
						childs[i]->draw(d, frustum, true);
			}
		}
}

void MRSection::draw(int d)
{
		if(d==level)
		{
				for(int i=0; i<num_splats; i++)
				{
					glNormal3f(splats[i].n[0], splats[i].n[1], splats[i].n[2]);
					glColor3f(splats[i].c[0], splats[i].c[1], splats[i].c[2]);
					glVertex3f(splats[i].vp[0], splats[i].vp[1], splats[i].vp[2]);
				}
		}
		else
		{
			for (int i=0; i<num_childs; i++) 
					childs[i]->draw(d);
		}
}

bool MRSection::is_front_facing(d_Frustum *frustum)
{
	if(ncone==NULL)
		return false;

	if(!frustum->changed)
		if(ncone->last_test_passed)
			return true;
		else
			return false;

	d_Vec3f tmp = norm(ncone->origin-frustum->camp);
	if(dot(cross(ncone->normal, cross(tmp, ncone->normal))*(-ncone->r)+ncone->normal*(ncone->d), tmp)>0)
		ncone->last_test_passed = false;
	else
		ncone->last_test_passed =  true;
	return ncone->last_test_passed;
}

MRSection::MRSection(int level, int depth)
{
	this->level = level;
	if(depth == 0)
		childs = NULL;
	else
	{
		num_childs = 4;
		childs = (MRSection **) malloc (num_childs*sizeof(MRSection *));
		for (int i=0; i<num_childs; i++) 
		{
			childs[i] = new MRSection(level+1, depth-1);
		}
	}
}

void MRSection::compute_splats(int bb_x, int bb_y, 
												int bb_w, int bb_h, 
												int shiftx, int shifty, 
												Image *p, Image *n, Image *c, 
												MRCamera cam, float dist, float dx)
{
	if(childs ==NULL)
	{
		num_splats =0;
		num_childs =0;
		for(int i=0; i<bb_w; i++)
			for(int j=0; j<bb_h; j++)
			{
				int u = i+shiftx;
				int v = j+shifty;
				if(p->GetR(u,v)==255)
					num_splats++;
			}
		if (num_splats==0) 
		{
			aabb = NULL;
			ncone =NULL;
			return;
		}
		splats = (Splat *) malloc (num_splats*sizeof(Splat));

		int splat_count = 0;

		for(int j=0; j<bb_h; j++)
			for(int i=0; i<bb_w; i++)
			{
				int u = i+shiftx;
				int v = j+shifty;
				if( p->GetR(u,v) == 255)
				{
					float treshold = (float)p->GetG(u,v)/255.0;
					d_Vec3f r= norm(cam.d*cam.focal + cam.xa*cam.sx*(-1)*(i-cam.cx+bb_x) + cam.ya*cam.sy*(j-cam.cy+bb_y));
					splats[splat_count].vp = cam.vp + r*(treshold*dx+dist);
					splats[splat_count].n = norm(d_Vec3f( (float)n->GetR(u,v)/255.0-0.5,  (float) n->GetG(u,v)/255.0-0.5, (float) n->GetB(u,v)/255.0-0.5));
					splats[splat_count].c = d_Vec3f((float)c->GetR(u,v)/255.0, (float) c->GetG(u,v)/255.0, (float) c->GetB(u,v)/255.0);
					splat_count++;
				}
			}
		compute_aabb();
		compute_ncone();
		return;
	}

	int w = bb_w;
	int h = bb_h;

	int size_w = (int)((float)w/2.0);
	int size_h = (int)((float)h/2.0);


	int counter =0;
	for(int i=0; i<2; i++)
		for(int j=0; j<2; j++)
		{
			int ex=0, ey=0;
			if(i==1)
				ex=bb_w%2;
			if(j==1)
				ey=bb_h%2;
			childs[counter]->compute_splats(bb_x+ i*size_w, 
																bb_y+ j*size_h, 
																size_w+ex, 
																size_h+ey, 
																shiftx+ i*(size_w),
																shifty+ j*(size_h), 
																p,n,c,cam,dist,dx);
			counter++;
		}

		num_splats=0;
		for(int i=0; i<num_childs; i++)
			if(childs[i]->num_splats>0)
				num_splats++;

		counter =0;
		MRSection **new_childs;// = (MRSection **) malloc (num_splats*sizeof(MRSection *));

		//(num_splats>1)
		{
			new_childs = (MRSection **) malloc (num_splats*sizeof(MRSection *));
			for(int i=0; i<num_childs; i++)
				if(childs[i]->num_splats>0)
				{
					new_childs[counter]=childs[i];
					counter++;
				}
			childs = new_childs;
			num_childs = num_splats;
		}
		/*
		if(num_splats ==0)
		{
			num_childs=0;
			childs =NULL;
		}

		if(num_splats==1)
		{
			num_splats=0;
			for(int i=0; i<childs[0]->num_childs; i++)
				if(childs[0]->childs[i]->num_splats>0)
					num_splats++;
			
			new_childs = (MRSection **) malloc (num_splats*sizeof(MRSection *));
			counter=0;
			for(int i=0; i<childs[0]->num_childs; i++)
				if(childs[0]->childs[i]->num_splats>0)
				{
					new_childs[counter]=childs[0]->childs[i];
					counter++;
				}
			childs = new_childs;
			num_childs=num_splats;
		//	reconstruct_levels(level+1);
		}*/
		
		splats = (Splat *) malloc(num_splats*sizeof(Splat));

		for(int i=0; i<num_childs; i++)
		{
			splats[i] = childs[i]->get_average_splat();
		}

		compute_aabb();
		compute_ncone();
}

Splat MRSection::get_average_splat()
{
	Splat res;
	res.vp = res.n = res.c = d_Vec3f(0,0,0);
	for(int i=0; i<num_splats; i++)
	{
		res = res + splats[i];
	}
	res.n = norm(res.n);
	return res/(float)num_splats;
}

void MRSection::compute_aabb()
{
	d_Vec3f min = d_Vec3f(10000,10000,10000);
	d_Vec3f max = d_Vec3f(-10000,-10000,-10000);
	if(num_childs>0)
		for(int i=0; i<num_childs; i++)
		{
			if(childs[i]->aabb->min[0]<min[0])
				min[0] = childs[i]->aabb->min[0];
			if(childs[i]->aabb->max[0]>max[0])
				max[0] = childs[i]->aabb->max[0];
			if(childs[i]->aabb->min[1]<min[1])
				min[1] = childs[i]->aabb->min[1];
			if(childs[i]->aabb->max[1]>max[1])
				max[1] = childs[i]->aabb->max[1];
			if(childs[i]->aabb->min[2]<min[2])
				min[2] = childs[i]->aabb->min[2];
			if(childs[i]->aabb->max[2]>max[2])
				max[2] = childs[i]->aabb->max[2];
		}

	if(num_childs==0)
		for(int i=0; i<num_splats; i++)
		{
			if(splats[i].vp[0]<min[0])
				min[0] = splats[i].vp[0];
			if(splats[i].vp[0]>max[0])
				max[0] = splats[i].vp[0];
			if(splats[i].vp[1]<min[1])
				min[1] = splats[i].vp[1];
			if(splats[i].vp[1]>max[1])
				max[1] = splats[i].vp[1];
			if(splats[i].vp[2]<min[2])
				min[2] = splats[i].vp[2];
			if(splats[i].vp[2]>max[2])
				max[2] = splats[i].vp[2];
		}
	aabb = new d_AABB(min,max);
}

void MRSection::compute_ncone()
{
	if(num_splats==0)
	{
		ncone = NULL;
		return;
	}
	Splat tmp = get_average_splat();
	tmp.n = norm(tmp.n);
	float r=0;
	float height=1;
	for (int i=0; i<num_splats; i++) 
	{
		float d = dot(tmp.n,splats[i].n);
		float t = sqrt(1-pow(d,2));
		if(t>r)
		{
			r=t;
			height = d;	
		}		
	}
	ncone = new d_NormalCone(tmp.vp, tmp.n,r,height);
}

void MRSection::draw_normals()
{
	glEnd();
	glBegin(GL_LINES);
		for(int i=0; i<num_splats; i++)
		{
			glColor3f(0,1,0);
			glVertex3fv(splats[i].vp.p);
			glColor3f(1,0,0);
			glVertex3fv((splats[i].vp+splats[i].n).p);
		}
	glEnd();
	glBegin(GL_POINTS);
}

